copp\copp\copp2/
formulation.rs

1//! Problem data models and builders for second-order path parameterization.
2//!
3//! # Method identity
4//! This module defines validated formulation objects for:
5//! - **Time-Optimal Path Parameterization (TOPP2)**,
6//! - **Convex-Objective Path Parameterization (COPP2)**.
7//!
8//! # Discrete variables (shared notation)
9//! On a station grid with closed index interval `[idx_s_start, idx_s_final]`:
10//! - state profile is `a(s)=\dot{s}^2`;
11//! - boundary tuple is `a_boundary = (a_start, a_final)`;
12//! - station count is `s_len = idx_s_final - idx_s_start + 1`.
13//!
14//! # High-level pipeline
15//! 1. Construct [`Topp2ProblemBuilder`](crate::solver::topp2_ra::Topp2ProblemBuilder) or [`Copp2ProblemBuilder`](crate::solver::copp2_socp::Copp2ProblemBuilder) from caller data.
16//! 2. Run builder validation (index interval, bounds, objective compatibility).
17//! 3. Build immutable problem objects used by DP/optimization backends.
18
19use crate::copp::constraints::Constraints;
20use crate::copp::{CoppObjective, validate_copp2_objectives};
21use crate::diag::{CoppError, check_non_negative, check_s_interval_valid};
22use crate::robot::robot_core::{Robot, RobotBasic, RobotTorque};
23
24/// Formulated TOPP2 problem data.
25///
26/// # Fields
27/// - `constraints`: path-dependent kinematic/dynamic bounds on the selected interval;
28/// - `idx_s_interval`: closed station-index interval `[idx_s_start, idx_s_final]`;
29/// - `a_boundary`: endpoint state tuple `(a_start, a_final)`.
30pub struct Topp2Problem<'a> {
31    pub(crate) constraints: &'a Constraints,
32    pub(crate) idx_s_interval: (usize, usize),
33    pub(crate) a_boundary: (f64, f64),
34}
35
36impl<'a> Topp2Problem<'a> {
37    /// Return station count on the closed interval.
38    ///
39    /// For `[idx_s_start, idx_s_final]`, this returns
40    /// `idx_s_final - idx_s_start + 1`.
41    #[inline]
42    pub fn s_len(&self) -> usize {
43        self.idx_s_interval.1 - self.idx_s_interval.0 + 1
44    }
45}
46
47/// Builder for [`Topp2Problem`](crate::solver::topp2_ra::Topp2Problem).
48pub struct Topp2ProblemBuilder<'a> {
49    /// Reference to path constraints.
50    pub constraints: &'a Constraints,
51    /// Closed station-index interval `(idx_s_start, idx_s_final)`.
52    pub idx_s_interval: (usize, usize),
53    /// Endpoint state tuple `(a_start, a_final)`.
54    pub a_boundary: (f64, f64),
55}
56
57impl<'a> Topp2ProblemBuilder<'a> {
58    /// Create a TOPP2 builder from a [`Robot`](crate::robot::Robot) reference.
59    ///
60    /// # Parameters
61    /// - `robot`: robot wrapper with the trait [`RobotBasic`](crate::robot::RobotBasic) whose constraint buffer defines the problem domain.
62    /// - `idx_s_interval`: closed station-index interval `(idx_s_start, idx_s_final)`.
63    /// - `a_boundary`: endpoint state tuple `(a_start, a_final)`.
64    #[inline]
65    pub fn new<M: RobotBasic>(
66        robot: &'a Robot<M>,
67        idx_s_interval: (usize, usize),
68        a_boundary: (f64, f64),
69    ) -> Self {
70        Self {
71            constraints: &robot.constraints,
72            idx_s_interval,
73            a_boundary,
74        }
75    }
76
77    /// Create a TOPP2 builder with all required fields.
78    ///
79    /// # Parameters
80    /// - `constraints`: reference to path constraints defining the problem domain.
81    /// - `idx_s_interval`: closed station-index interval `(idx_s_start, idx_s_final)`.
82    /// - `a_boundary`: endpoint state tuple `(a_start, a_final)`.
83    #[inline]
84    pub fn with_constraint(
85        constraints: &'a Constraints,
86        idx_s_interval: (usize, usize),
87        a_boundary: (f64, f64),
88    ) -> Self {
89        Self {
90            constraints,
91            idx_s_interval,
92            a_boundary,
93        }
94    }
95
96    /// Build a validated [`Topp2Problem`](crate::solver::topp2_ra::Topp2Problem).
97    #[inline]
98    pub fn build(&self) -> Result<Topp2Problem<'a>, CoppError> {
99        self.validate()?;
100        Ok(Topp2Problem {
101            constraints: self.constraints,
102            idx_s_interval: self.idx_s_interval,
103            a_boundary: self.a_boundary,
104        })
105    }
106
107    /// Validate builder fields and consistency.
108    #[inline]
109    pub fn validate(&self) -> Result<(), CoppError> {
110        check_s_interval_valid(
111            "Topp2ProblemBuilder",
112            self.idx_s_interval.0,
113            self.idx_s_interval.1,
114        )?;
115        self.constraints.check_s_in_bounds(
116            self.idx_s_interval.0,
117            self.idx_s_interval.1 - self.idx_s_interval.0 + 1,
118        )?;
119        check_non_negative(
120            "Topp2ProblemBuilder",
121            "a_start (a_boundary.0)",
122            self.a_boundary.0,
123        )?;
124        check_non_negative(
125            "Topp2ProblemBuilder",
126            "a_final (a_boundary.1)",
127            self.a_boundary.1,
128        )?;
129        Ok(())
130    }
131}
132
133/// Formulated COPP2 problem data.
134///
135/// # Fields
136/// - `robot`: robot model supplying constraints and torque-related terms;
137/// - `objectives`: objective list for COPP2 optimization;
138/// - `idx_s_interval`: closed station-index interval `[idx_s_start, idx_s_final]`;
139/// - `a_boundary`: endpoint state tuple `(a_start, a_final)`.
140pub struct Copp2Problem<'a, M: RobotTorque> {
141    pub(crate) robot: &'a Robot<M>,
142    pub(crate) objectives: &'a [CoppObjective<'a>],
143    pub(crate) idx_s_interval: (usize, usize),
144    pub(crate) a_boundary: (f64, f64),
145}
146
147impl<'a, M: RobotTorque> Copp2Problem<'a, M> {
148    /// Return station count on the closed interval.
149    ///
150    /// For `[idx_s_start, idx_s_final]`, this returns
151    /// `idx_s_final - idx_s_start + 1`.
152    #[inline]
153    pub fn s_len(&self) -> usize {
154        self.idx_s_interval.1 - self.idx_s_interval.0 + 1
155    }
156}
157
158/// Builder for [`Copp2Problem`](crate::solver::copp2_socp::Copp2Problem).
159pub struct Copp2ProblemBuilder<'a, M: RobotTorque> {
160    /// Reference to robot model defining constraints and dynamics.
161    pub robot: &'a Robot<M>,
162    /// Closed station-index interval `(idx_s_start, idx_s_final)`.
163    pub idx_s_interval: (usize, usize),
164    /// Endpoint state tuple `(a_start, a_final)`.
165    pub a_boundary: (f64, f64),
166    /// Objectives for COPP2 optimization.
167    pub objectives: &'a [CoppObjective<'a>],
168}
169
170impl<'a, M: RobotTorque> Copp2ProblemBuilder<'a, M> {
171    /// Create a COPP2 builder with all required fields.
172    ///
173    /// # Parameters
174    /// - `robot`: robot with the trait [`RobotTorque`](crate::robot::RobotTorque) defining the problem domain.
175    /// - `idx_s_interval`: closed station-index interval `(idx_s_start, idx_s_final)`.
176    /// - `a_boundary`: endpoint state tuple `(a_start, a_final)`.
177    #[inline]
178    pub fn new(
179        robot: &'a Robot<M>,
180        idx_s_interval: (usize, usize),
181        a_boundary: (f64, f64),
182        objectives: &'a [CoppObjective<'a>],
183    ) -> Self {
184        Self {
185            robot,
186            idx_s_interval,
187            a_boundary,
188            objectives,
189        }
190    }
191
192    /// Build a validated [`Copp2Problem`](crate::solver::copp2_socp::Copp2Problem).
193    #[inline]
194    pub fn build(&self) -> Result<Copp2Problem<'a, M>, CoppError> {
195        self.validate()?;
196        Ok(Copp2Problem {
197            robot: self.robot,
198            idx_s_interval: self.idx_s_interval,
199            a_boundary: self.a_boundary,
200            objectives: self.objectives,
201        })
202    }
203
204    /// Validate builder fields and objective compatibility.
205    #[inline]
206    pub fn validate(&self) -> Result<(), CoppError> {
207        check_s_interval_valid(
208            "Copp2ProblemBuilder",
209            self.idx_s_interval.0,
210            self.idx_s_interval.1,
211        )?;
212        self.robot.constraints.check_s_in_bounds(
213            self.idx_s_interval.0,
214            self.idx_s_interval.1 - self.idx_s_interval.0 + 1,
215        )?;
216        check_non_negative(
217            "Copp2ProblemBuilder",
218            "a_start (a_boundary.0)",
219            self.a_boundary.0,
220        )?;
221        check_non_negative(
222            "Copp2ProblemBuilder",
223            "a_final (a_boundary.1)",
224            self.a_boundary.1,
225        )?;
226
227        let s_len = self.idx_s_interval.1 - self.idx_s_interval.0 + 1;
228        validate_copp2_objectives(
229            "Copp2ProblemBuilder",
230            self.objectives,
231            self.robot.dim(),
232            s_len,
233        )?;
234        Ok(())
235    }
236}
237
238impl<'a, M: RobotTorque> Copp2Problem<'a, M> {
239    /// Convert to the TOPP2 view that shares interval and boundary fields.
240    ///
241    /// This is used by internal stages that only need standard TOPP2 constraints.
242    pub(crate) fn as_topp2_problem(&self) -> Topp2Problem<'a> {
243        Topp2Problem {
244            constraints: &self.robot.constraints,
245            idx_s_interval: self.idx_s_interval,
246            a_boundary: self.a_boundary,
247        }
248    }
249}
copp\copp\copp2/formulation.rs

copp\copp\copp2/
formulation.rs
